#include <fstream>
#include "GenPoints.h"
#include "Fitting.h"

using namespace rock;

auto load_data(std::string path) -> points_type {
	points_type all_points = {};

	std::ifstream if_data(path, std::ios::in);
	if (if_data.is_open()) {
		std::string s;
		while (!if_data.eof()) {
			getline(if_data, s);
			if (s.empty()) continue;
			if (s[0] == '#') {
				std::cout << s << std::endl;
				continue;
			}

			std::stringstream ss;
			ss << s;

			point_type p = {};
			ss >> p[0];
			ss >> p[1];
			ss >> p[2];
			all_points.emplace_back(p);
		}
		if_data.close();
	}
	return all_points;
}


int main() {
	/******* generate point cloud ********/
//	plane_type gt {0.8, 0.2, 1.0, 0.5};
//	GenPoints   genPoints(gt, 1.5);
//	points_type pts = genPoints.generate();
//	genPoints.save(pts);

	/******* load point cloud ********/
	points_type pts = load_data("../test/points.txt");

	Fitting fitting(pts);
	auto    ls_plane     = fitting.least_sqaure();
	auto    ransac_plane = fitting.ransac();

	std::cout<<"ls_plane: "<<ls_plane.transpose()<<std::endl;
	std::cout<<"ransac_plane: "<<ransac_plane.transpose()<<std::endl;

	return 0;
}
